manifold_wrapper.h

Go to the documentation of this file.

// WOLF - Copyright (C) 2020-2026
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
 
// Fwd refs
namespace wolf
{
class LocalParametrizationBase;
}
 
// Ceres includes
#include "core/common/wolf.h"
#include "ceres/ceres.h"
 
namespace wolf
{
class ManifoldWrapper : public ceres::Manifold
{
  private:
    LocalParametrizationBasePtr local_parametrization_ptr_;
 
  public:
    ManifoldWrapper(LocalParametrizationBasePtr _local_parametrization_ptr);
 
    ~ManifoldWrapper() override = default;
 
    bool Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const override;
 
    bool PlusJacobian(const double* x, double* jacobian) const override;
 
    bool Minus(const double* x_raw, const double* y_raw, double* x_minus_y_raw) const override;
 
    bool MinusJacobian(const double* x, double* jacobian) const override;
 
    int AmbientSize() const override;
 
    int TangentSize() const override;
 
    LocalParametrizationBasePtr getLocalParametrization() const;
};
 
using ManifoldWrapperPtr = std::shared_ptr<ManifoldWrapper>;
 
}  // namespace wolf
 
#include "core/state_block/local_parametrization_base.h"
 
namespace wolf
{
inline ManifoldWrapper::ManifoldWrapper(LocalParametrizationBasePtr _local_parametrization_ptr)
    : local_parametrization_ptr_(_local_parametrization_ptr)
{
}
 
inline int ManifoldWrapper::AmbientSize() const
{
    return local_parametrization_ptr_->getGlobalSize();
}
 
inline int ManifoldWrapper::TangentSize() const
{
    return local_parametrization_ptr_->getLocalSize();
}
 
inline LocalParametrizationBasePtr ManifoldWrapper::getLocalParametrization() const
{
    return local_parametrization_ptr_;
}
 
}  // namespace wolf